#include "KdTree.h"

int compareX (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	float x1 = (b1.bounds[0].max + b1.bounds[0].min)/2;
	float x2 = (b2.bounds[0].max + b2.bounds[0].min)/2;
	return (int) (x1 - x2);
}

int compareY (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	float y1 = (b1.bounds[1].max + b1.bounds[1].min)/2;
	float y2 = (b2.bounds[1].max + b2.bounds[1].min)/2;
	return (int) (y1 - y2);
}

int compareZ (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	float z1 = (b1.bounds[2].max + b1.bounds[2].min)/2;
	float z2 = (b2.bounds[2].max + b2.bounds[2].min)/2;
	return (int) (z1 - z2);
}

int compareXMIN (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	return (int)( b1.bounds[0].min - b2.bounds[0].min);
}

int compareXMAX (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	return (int)( b1.bounds[0].max - b2.bounds[0].max);
}

int compareYMIN (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	return (int)( b1.bounds[1].min - b2.bounds[1].min);
}

int compareYMAX (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	return (int)( b1.bounds[1].max - b2.bounds[1].max);
}

int compareZMIN (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	return (int)( b1.bounds[2].min - b2.bounds[2].min);
}

int compareZMAX (const void * a, const void * b)
{
	BB_t b1 = *(BB_t*)a;
	BB_t b2 = *(BB_t*)b;
	return (int)( b1.bounds[2].max - b2.bounds[2].max);
}

aiVector3D cross(const aiVector3D &a, const aiVector3D &b)
{
	aiMatrix3x3 skewMatrix = aiMatrix3x3(0, -a.z, a.y, a.z, 0, -a.x, -a.y, a.x, 0);
	return (skewMatrix*b);
}

aiVector3D triNormal(const triangle_t &t)
{
	aiVector3D a = t.vertices[1] - t.vertices[0];
	aiVector3D b = t.vertices[2] - t.vertices[0];
	// cross product again
	aiVector3D n = (cross(a,b)).Normalize();

	return n;
}

KdTree::KdTree() {
	_root = new kd_node_t;

}

void KdTree::genAllTriangles(const aiScene* scene, const aiNode* nd) {

        modelMatrixStack.empty();
	modelMatrix = Identity;
	genTriangleList(scene, nd);
}

void KdTree::setMeshIndices(const aiMesh* mesh){

	// Set up the index buffer.  Each face should have 3 vertices since we
	// specified aiProcess_Triangulate
	indexBuffer.clear();
	indexBuffer.reserve(mesh->mNumFaces * 3);
	for (unsigned i = 0; i < mesh->mNumFaces; i++) {
		for (unsigned j = 0; j < mesh->mFaces[i].mNumIndices; j++) {
			indexBuffer.push_back(mesh->mFaces[i].mIndices[j]);
		}
	}

}

BB_t KdTree::computeTriBB(triangle_t& t)
{
	BB_t bb;

	// sort the x in vertices of t
	float xArr[3] = {t.vertices[0].x, t.vertices[1].x, t.vertices[2].x};
	std::sort(xArr, xArr+3);
	bb.bounds[0].min = xArr[0]; 
	bb.bounds[0].max = xArr[2];
	// sort the y in vertices of t
	float yArr[3] = {t.vertices[0].y, t.vertices[1].y, t.vertices[2].y};
	std::sort(yArr, yArr+3);
	bb.bounds[1].min = yArr[0]; 
	bb.bounds[1].max = yArr[2];
	//sort the z in vertices of t
	float zArr[3] = {t.vertices[0].z, t.vertices[1].z, t.vertices[2].z};
	std::sort(zArr, zArr+3);
	bb.bounds[2].min = zArr[0]; 
	bb.bounds[2].max = zArr[2];

	return bb;
}
/**
  * pushTriangeBB push the triangle in the mesh into a global, giant, triangle list 
  */
void KdTree::pushTriangleBB(aiMesh *mesh, const aiMatrix4x4& modelMatrix)
{
	int triangleN = mesh->mNumFaces;
	triangle_list.reserve(triangleN);
	for (int i=0; i< triangleN; i++){
		// generate a traingle struct
		triangle_t t;
		for (int k=0; k<3; k++){
			// fetch vertices from the index buffer over
			int ind = indexBuffer[3*i + k];
			aiVector3D vertice = mesh->mVertices[ind];
			vertice = modelMatrix*vertice;// transform it to the world space
			t.vertices[k] = vertice;
		}
		// computing the BB
		t.bb = computeTriBB(t);
		t.bb.t_index = triangle_list.size(); // each bounding box knows the triangle index
		triangle_list.push_back(t);

	}

}
void KdTree::genTriangleList(const aiScene *scene, const aiNode* node) {

	modelMatrixStack.push(modelMatrix);
	aiMatrix4x4 aiMat = node->mTransformation;// row major
     	modelMatrix *= aiMat; // row major
	 
	aiMesh* mesh;
	for (unsigned k = 0; k< node->mNumMeshes; k++){// scene->mNumMeshes
		// specified aiProcess_Triangulate
		mesh = scene->mMeshes[node->mMeshes[k]];
		if (mesh->mPrimitiveTypes != aiPrimitiveType_TRIANGLE) continue;
 
		// you need the indexBuffer; that's basically it.
		setMeshIndices(mesh);
		pushTriangleBB(mesh, modelMatrix);
	
	}
	// do the children
	for (unsigned ch = 0; ch< node->mNumChildren; ch++){
		genTriangleList(scene, node->mChildren[ch]);
	}

	modelMatrix = modelMatrixStack.top();
	modelMatrixStack.pop();

}


BB_t KdTree::compute_world_bbox(const BB_t* wbboxArr, unsigned size)
{
	BB_t retbox;
	BB_t* arr = new BB_t[size];
	memcpy(arr, wbboxArr, size*sizeof(BB_t));

	qsort(arr, size, sizeof(BB_t), compareXMIN);
	retbox.bounds[0].min = arr[0].bounds[0].min;

	qsort(arr, size, sizeof(BB_t), compareXMAX);
	retbox.bounds[0].max = arr[size-1].bounds[0].max;

	qsort(arr, size, sizeof(BB_t), compareYMIN);
	retbox.bounds[1].min = arr[0].bounds[1].min;

	qsort(arr, size, sizeof(BB_t), compareYMAX);
	retbox.bounds[1].max = arr[size-1].bounds[1].max;

	qsort(arr, size, sizeof(BB_t), compareZMIN);
	retbox.bounds[2].min = arr[0].bounds[2].min;

	qsort(arr, size, sizeof(BB_t), compareZMAX);
	retbox.bounds[2].max = arr[size-1].bounds[2].max;

	delete [] arr;
	return retbox;
}

bool KdTree::bboxIsTooSmall(const BB_t &bbox)
{
	float vol = bbox.xlen() * bbox.ylen()* bbox.zlen();
	if (vol < 0.001) return true;
	return false;
}

void KdTree::split(kd_node_t* me, BB_t* bboxArr, size_t bboxn)
{
	static unsigned dim = 0;
 
	// determine which dim. to split!	
	dim = (dim+1)%3;

	// terminate condition 2
	if ( (bboxn < 50) && (bboxIsTooSmall(me->bbox))){
	  //if (bboxn < 50){
		// it is a leaf node!
		// contain a list of bounding boxes!
		me->leaf_bboxes = bboxArr;
		me->leaf_bboxn = bboxn;
		return;
	}
	
	//dim = findLongestDim(me->bbox); // for some reason this does not work.

	// switch the function for sorting along different dim.
	int (*pt2Func)(const void*, const void*);
        
	switch(dim){
		case 0:
			pt2Func = compareX;
			break;
		case 1:
			pt2Func = compareY;
			break;
		case 2:
			pt2Func = compareZ;
			break;
	}
	
	// find median #
	size_t median;
	if (bboxn%2 == 0){
		median = bboxn/2;
	}else{
		median = (bboxn + 1)/2;
	}

	// sort the bboxArr along dim. direction and take the median out for the splitting plane. 
	qsort(bboxArr, bboxn, sizeof(BB_t), pt2Func);
	// find where to split
	float median_max = bboxArr[median].bounds[dim].max;
	float median_min = bboxArr[median].bounds[dim].min;
	float split_loc = (median_max + median_min)/2; // is the location to split
       	 
	std::vector<BB_t> lbboxBuf;
	std::vector<BB_t> rbboxBuf;
	 
	// assigning the bbox to different bufffer
	for (size_t i=0; i< bboxn; i++){
		BB_t bb = bboxArr[i];
		if (bb.bounds[dim].max < split_loc){
			// belong to the left node
			lbboxBuf.push_back(bb);
		}else if(bb.bounds[dim].min > split_loc){
			// belong to the right node
			rbboxBuf.push_back(bb);
		}else{ // intersect!
			// generate a new bb to the left list
			BB_t lbb;
			lbb.copy(&bb);
			lbb.bounds[dim].max = split_loc;
			lbboxBuf.push_back(lbb);
			// generate a new bb to the right list
			BB_t rbb;
			rbb.copy(&bb);
			rbb.bounds[dim].min = split_loc;
			rbboxBuf.push_back(rbb);
		}
	}	 

	size_t rsize = rbboxBuf.size();
	size_t lsize = lbboxBuf.size();
	
	// terminate condition 3
	if ( ((rsize == bboxn) || (lsize == bboxn)) && (bboxn < 1000)) {
		// it is a leaf node!
		// contain a list of bounding boxes!
		me->leaf_bboxes = bboxArr;
		me->leaf_bboxn = bboxn;
 
		return;
	}
	
	delete [] bboxArr;

	BB_t* rbboxArr = new BB_t[rsize];
	BB_t* lbboxArr = new BB_t[lsize];

	for (size_t k=0; k<rsize; k++){
		rbboxArr[k] = rbboxBuf[k];
	}
	for (size_t k=0; k<lsize; k++){
		lbboxArr[k] = lbboxBuf[k];
	}

	me->split_dim = dim;
	me->split_loc = split_loc;

	// create l_node
	kd_node_t* l_child = new kd_node_t;
	l_child->bbox.copy(&me->bbox);
	l_child->bbox.bounds[dim].max = split_loc;
	me->l_child = l_child;

	// create r_node
	kd_node_t* r_child = new kd_node_t;
	r_child->bbox.copy(&me->bbox);
	r_child->bbox.bounds[dim].min = split_loc;
	me->r_child = r_child;	 

	// recursive
	split(l_child, lbboxArr, lsize);
	split(r_child, rbboxArr, rsize);
		
}

void KdTree::build_tree(){
	// take the triangle_list, create an array to store all the bbox
	unsigned int tn = triangle_list.size();
	BB_t* wbboxArr = new BB_t [tn];
	for (unsigned i=0 ; i< tn; i++){
		wbboxArr[i] = triangle_list[i].bb;
	}

	_root->bbox = compute_world_bbox(wbboxArr, tn);
	
	// build the kdtree
	split(_root, wbboxArr, tn); //within this function; wbboxArr will be de-allocated.

}

void KdTree::triangle_intersect(const line_t &line, BB_t* bbItemList, unsigned bbItemn)
{
	for (unsigned i= 0; i<bbItemn; i++)
	{
		unsigned t_ind = bbItemList[i].t_index;
		triangle_t tri = triangle_list[t_ind];
		// calculate triangle normal
		aiVector3D n = triNormal(tri);
                
		// Now that the line p1 is "earlier" 
		// find intersection point
		aiVector3D d = line.lineVec();
	
                //printf("dx = %f, dy = %f, dz = %f\n", d.x, d.y, d.z);
                //printf("nx = %f, ny = %f, nz = %f\n", n.x, n.y, n.z);
                //printf("d*n = %f\n", d*n);
	
                
		// test 1
		if (d*n > 0) {// test the next triangle
                  aiVector3D flip = aiVector3D(-n.x, -n.y, -n.z);
		  n = flip;
                 
                  // test 2.2
                  aiVector3D v1 =  cross(tri.vertices[2]- line.p1, tri.vertices[0]- line.p1); 
                  if (!(v1*d > 0)) continue;
                  // test 3.2
                  aiVector3D v2 =  cross(tri.vertices[1]- line.p1, tri.vertices[2]- line.p1);
                  if (!(v2*d > 0)) continue;
		  // test 4.2
		  aiVector3D v3 =  cross(tri.vertices[0]- line.p1, tri.vertices[1] - line.p1);
                  if (!(v3*d > 0)) continue;

                } else{
                
		  //if (d*n > 0) continue; 

		// test 2
		aiVector3D v1 = cross(tri.vertices[1]- line.p1, tri.vertices[0]- line.p1);
		if (!(v1*d > 0)) continue;
		// test 3
		aiVector3D v2 = cross(tri.vertices[2]- line.p1, tri.vertices[1]- line.p1);
		if (!(v2*d > 0)) continue;
		// test 4
		aiVector3D v3 = cross(tri.vertices[0] - line.p1, tri.vertices[2] - line.p1);
		if (!(v3*d > 0)) continue;
		}
                
		// test 5 solve for the intersection point, i.e, parameter t
		aiVector3D a_ro = (tri.vertices[0] - line.p1); // a - r0
		float t = (a_ro*n)/(d*n);
		if (t>1.0f || t<0.0f) continue; // the line segment does not intersect with the tri.

		// now passed all the test; we confirmed that this triangle intersect

		// list all the pts that intersect!
		aiVector3D p = line.p1 + t*d;
                
                
		intersectPts.push_back(p);
		// intersectTriNormal.push_back(n);

	}
	
}

/** I am a recursive function
  * who take in info for a line segment and aims to find the right b-box, do triangle- line 
  * intersection, etc.
  */
void KdTree::collide_kd_tree(kd_node_t* me , const line_t &line )
{
	// is leaf node
	if (me->leaf_bboxes){
		// perform triangle-intersection test
		triangle_intersect(line, me->leaf_bboxes, me->leaf_bboxn); 
		return;
	}
	
	unsigned dim = me->split_dim;
     	float split_loc = me->split_loc;


        aiVector3D nl = line.lineVec();
	nl.Normalize();
      
	float ratio;
	bool divide_by_zero = false;

        float thres = 0.001f;
        float ix, iy, iz;

	switch(dim){
		case 0:
		{
			if (nl.x < thres) 
				divide_by_zero = true;
			else{
			  ratio = (split_loc - line.p1.x)/nl.x ;
			  ix = split_loc;
			  iy = line.p1.y + ratio*nl.y;
			  iz = line.p1.z + ratio*nl.z;
			} 
			break;
		}
		case 1:
		{
			if (nl.y < thres)
				divide_by_zero = true;
			else{
			  ratio = (split_loc - line.p1.y)/nl.y;
			  ix = line.p1.x + ratio*nl.x;
                          iy = split_loc;
			  iz = line.p1.z + ratio*nl.z;
			} 
			break;
		}
		case 2:
		{ 
			if (nl.z < thres)
				divide_by_zero = true;
			else{
			  ratio = (split_loc - line.p1.z)/nl.z;
                          ix = line.p1.x + ratio*nl.x;
                          iy = line.p1.y + ratio*nl.y;
			  iz = split_loc;
                        } 
			break;
		}

	}

         

	if (line.getMin(dim) >= split_loc){
		// go to the right node
		collide_kd_tree(me->r_child, line);
	} else if (line.getMax(dim) < split_loc){
		// go to the left node
		collide_kd_tree(me->l_child, line);
	} else{

		if (divide_by_zero){
			collide_kd_tree(me->l_child, line);
			return;
		}
		// intersect; need split
	
		aiVector3D interp = aiVector3D(ix, iy, iz);
 
		line_t l_seg;
		line_t r_seg;

		if (line.p1[dim] < line.p2[dim]){

			// when you pass it down the tree, make sure p1 is always closer to the original p1????
			l_seg.p1 = line.p1; 
			l_seg.p2 = interp;
				
			r_seg.p1 = interp;
			r_seg.p2 = line.p2;
		} else {
			//l_seg.p1 = line.p2; 
			//l_seg.p2 = interp;
			l_seg.p1 = interp;
			l_seg.p2 = line.p2;
				
			//r_seg.p1 = interp;
			//r_seg.p2 = line.p1;
			r_seg.p1 = line.p1;
			r_seg.p2 = interp;
		}
       
		collide_kd_tree(me->l_child, l_seg);
		collide_kd_tree(me->r_child, r_seg);

	}

}

/** 
  * Determine if the camera collide with tri. Update the cameraPos accordingly.
  */
bool KdTree::cameraCollisionTest(const aiVector3D &camPos, const aiVector3D &updateVec)
{
		//static float delta = 0.1;
		
		line_t line;
		aiVector3D dn = updateVec;
		dn.Normalize();

                float camRadius = 0.1f;

              	aiVector3D startPos = camPos + camRadius*dn;
		aiVector3D endPos = camPos + updateVec + camRadius*dn; 

		//printf("startPos = %f, %f, %f\n", startPos.x, startPos.y, startPos.z);
                //printf("endPos= %f, %f, %f\n", endPos.x, endPos.y, endPos.z);

		line.init(startPos, endPos);

		intersectPts.clear();
		//      printf("before collide_kd_tree\n");
        	collide_kd_tree(_root, line);
	

		if (intersectPts.empty()){ // no collision
		      return false;
		}
		
               
		// printf("camera collides\n"); 
                return true;
		//camPos = camPos - delta*dn;
                                
}

bool KdTree::characterCollisionTest(const aiVector3D &camPos, const aiVector3D &updateVec)
{
		//static float delta = 0.1;
		
		line_t line;
		aiVector3D dn = updateVec;
		dn.Normalize();

                float camRadius = 0.1f;

              	aiVector3D startPos = camPos + camRadius*dn;
		aiVector3D endPos = camPos + updateVec + camRadius*dn; 

		line.init(startPos, endPos);

		const float width = 2.5;
		aiVector3D right = cross(updateVec, aiVector3D(0.0f,1.0f,0.0f));
		right.Normalize();
		aiVector3D left = - right;

		line_t lineLeft;
		line_t lineRight;
		lineLeft.init(startPos, startPos + right*width);
		lineRight.init(startPos, startPos + left*width);
	
		intersectPts.clear();
		//      printf("before collide_kd_tree\n");
        	collide_kd_tree(_root, line);
		//collide_kd_tree(_root, lineLeft);
		//collide_kd_tree(_root, lineRight);

		if (intersectPts.empty()){ // no collision
		      return false;
		}
		
		//printf("character colli.\n");
                return true;
		
                                
}

// debug function
void KdTree::drawCube(const BB_t &bbox)
{
	//printf("inside drawCube!\n");
	GLfloat vertices[] = {
	bbox.bounds[0].min, bbox.bounds[1].min, bbox.bounds[2].min,
	bbox.bounds[0].max, bbox.bounds[1].min, bbox.bounds[2].min,
	bbox.bounds[0].max, bbox.bounds[1].max, bbox.bounds[2].min,
	bbox.bounds[0].min, bbox.bounds[1].max, bbox.bounds[2].min,
	bbox.bounds[0].min, bbox.bounds[1].min, bbox.bounds[2].max,
	bbox.bounds[0].max, bbox.bounds[1].min, bbox.bounds[2].max,
	bbox.bounds[0].max, bbox.bounds[1].max, bbox.bounds[2].max,
	bbox.bounds[0].min, bbox.bounds[1].max, bbox.bounds[2].max,
	};

	/*
	GLuint indices[] = { 4, 5, 6, 7, 1, 2, 6, 5,
						 0, 1, 5, 4, 0, 3, 2, 1,
						 0, 4, 7, 3, 2, 3, 7, 6};
	glColor3f(1.0, 0.0, 0.0);
	glLineWidth(1.0);
	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_FLOAT, 0, vertices);//stride = 0; tightly packed!
	GL_CHECK(glDrawElements(GL_QUADS, 24, GL_UNSIGNED_INT, indices)); 
	glDisableClientState(GL_VERTEX_ARRAY);
	*/
	
	GLuint indices[] = { 0, 1, 1, 2, 2, 3, 3, 0,
						 4, 5, 5, 6, 6, 7, 7, 4,
						 2, 6, 3, 7, 1, 5, 0, 4};

	glColor3f(1.0, 0.0, 0.0);
	glLineWidth(0.5);
	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_FLOAT, 0, vertices);//stride = 0; tightly packed!
	glDrawElements(GL_LINES, 24, GL_UNSIGNED_INT, indices); 
	glDisableClientState(GL_VERTEX_ARRAY);
	
}

//debug version
void KdTree::render_kd_tree(kd_node_t* t_node, unsigned &itemn){

	if (t_node->leaf_bboxes){
		// draw the cube!
		//itemn += t_node->leaf_bboxn;
		if (t_node->leaf_bboxn > itemn) itemn = t_node->leaf_bboxn;
		drawCube(t_node->bbox);
		return;
	}
	render_kd_tree(t_node->l_child, itemn);
	render_kd_tree(t_node->r_child, itemn);

}  
